//  Copyright (C) 2012 Alexander Alimov
//
//	This file is part of Lodestar library.
//
//		Lodestar library is free software: you can redistribute it and/or modify
//		it under the terms of the GNU Lesser General Public License as published by
//		the Free Software Foundation, either version 3 of the License, or
//		(at your option) any later version.
//
//		Lodestar library is distributed in the hope that it will be useful,
//		but WITHOUT ANY WARRANTY; without even the implied warranty of
//		MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//		GNU Lesser General Public License for more details.
//
//		You should have received a copy of the GNU Lesser General Public License
//		along with Lodestar library.  If not, see <http://www.gnu.org/licenses/>.
#include "config.h"
#include "steering.h"
#include <cmath>
#include <algorithm>

namespace lodestar
{

    Vector SeekSteering::getSteering()
    {
        Vector desizredVelocity = (mTarget - mVehicle->getPosition()).normalize() * mVehicle->getMaxSpeed();
        Vector steering = desizredVelocity - mVehicle->getVelocity();

        return steering;
    }

    bool SeekSteering::init( IVehicle* vehicle, const Vector& target )
    {
        mVehicle = vehicle; mTarget = target; return true;
    }

    Vector FleeSteering::getSteering()
    {
        Vector desizredVelocity = (mVehicle->getPosition() - mTarget).normalize() * mVehicle->getMaxSpeed();
        Vector steering = desizredVelocity - mVehicle->getVelocity();

        return steering;
    }

    Vector PursuitSteering::getSteering()
    {
        Vector predictedTargetPosition = mTarget->getPosition();
        Vector desizredVelocity = (predictedTargetPosition - mVehicle->getPosition()).normalize() * mVehicle->getMaxSpeed();
        Vector steering = desizredVelocity - mVehicle->getVelocity();

        return steering;
    }

    Vector EvasionSteering::getSteering()
    {
        Vector predictedTargetPosition = mTarget->getPosition();
        Vector desizredVelocity = (mVehicle->getPosition() - predictedTargetPosition).normalize() * mVehicle->getMaxSpeed();
        Vector steering = desizredVelocity - mVehicle->getVelocity();

        return steering;

    }

    Vector ArrivalSteering::getSteering()
    {
        Vector targetOffset = mTarget - mVehicle->getPosition();
        float distance = targetOffset.length();
        float rampedSpeed = mVehicle->getMaxSpeed() * (distance / mSlowingDistance);
        float clippedSpeed = std::min(rampedSpeed, mVehicle->getMaxSpeed());
        Vector desiredVelocity = targetOffset * (clippedSpeed / distance);
        Vector steering = desiredVelocity - mVehicle->getVelocity();

        return steering;
    }

    bool ArrivalSteering::init( IVehicle* vehicle, const Vector& target, float distance )
    {
        mVehicle = vehicle; 
        mTarget = target; 
        mSlowingDistance = distance;
        return true;
    }

    Vector ObstacleAvoidanceSteering::getSteering()
    {
        Vector steering;
        return steering;
    }

    Vector WanderSteering::getSteering()
    {
        Vector steering;
        return steering;
    }

    Vector ExploreSteering::getSteering()
    {
        Vector steering;
        return steering;
    }

    Vector PathFollowingSteering::getSteering()
    {
        Vector steering;
        return steering;
    }


    Vector WheightedSteering::getSteering()
    {
        Vector steering;
        SteeringComponent* component = mSteerings;
        while (component != nullptr)
        {
            Vector c = component->steering->getSteering();
            c *= component->wheight;
            steering += c;

            component = component->next;
        }

        return steering;
    }


    Vector PrioritizedDithering::getSteering()
    {
        Vector steering;
        return steering;
    }

}
